﻿using UnityEngine;
using System;
using System.Runtime.InteropServices;

public static class MathLib
{
    public const float PI2 = Mathf.PI * 2;
    public const float PI_Div2 = Mathf.PI * 0.5f;
    public const float PI_Div4 = Mathf.PI * 0.25f;
    public const float PI_Div8 = Mathf.PI * 0.125f;
    public readonly static float Sqrt2 = Mathf.Sqrt(2.0f);
    public readonly static float Sqrt2Half = Mathf.Sqrt(2.0f) * 0.5f;

    static IntOrFloat _ifVal = new IntOrFloat(0);

    [StructLayout(LayoutKind.Explicit)]
    struct IntOrFloat
    {
        [FieldOffset(0)]
        public Int32 i;
        [FieldOffset(0)]
        public UInt32 u;
        [FieldOffset(0)]
        public Single f;
        public IntOrFloat(int _i) {
            f = 0;
            u = 0;
            i = _i;
        }
        public IntOrFloat(float _f) {
            i = 0;
            u = 0;
            f = _f;
        }
    }
    
    public static Single FastAbs(Single fval) {
        _ifVal.f = fval;
        _ifVal.i &= 0x7FFFFFFF;
        return _ifVal.f;
    }

    public static Single Negate(Single fval) {
        _ifVal.f = fval;
        _ifVal.u ^= 0x80000000; // flip sign bit
        return _ifVal.f;
    }

    public static Int32 F2I(Single fval) {
        _ifVal.f = fval;
        return _ifVal.i;
    }

    public static Single I2F(Int32 ival) {
        _ifVal.i = ival;
        return _ifVal.f;
    }

    // compute sqrt(x)
    // assumes x >= 0
    public static Single FastSqrt1(Single fval) {
        //System.Diagnostics.Debug.Assert( fval >= 0 );
        _ifVal.f = fval;
        if (0 == ((_ifVal.i >> 23) & 255)) {
            return 0; // close
        }
        return fval * InvSqrt(fval);
    } // FastSqrt1
    
    // compute sqrt(x)
    // assumes x >= 0
    public static Single FastSqrt2(Single fval) {
        // System.Diagnostics.Debug.Assert( fval >= 0 );
        _ifVal.f = fval;
        _ifVal.i &= 0x7FFFFFFF;  // handle the -0 case, else may get garbage!
        _ifVal.i -= 0x3f800000; // subtract 127 from biased exponent
        _ifVal.i >>= 1; // requires signed shift to preserve sign
        _ifVal.i += 0x3f800000; // rebias the new exponent
        return _ifVal.f;
    }
    
    // compute 1/sqrt(x)
    // assumes x > 0
    public static Single InvSqrt(Single x) {
        // to see why works, read paper on www.lomont.org
        float xhalf = 0.5f * x;
        _ifVal.f = x;
        _ifVal.i = 0x5f375a86 - (_ifVal.i >> 1); // gives initial guess y0 with magic number
        return _ifVal.f * (1.5f - xhalf * _ifVal.f * _ifVal.f);  // Newton step, repeating increases accuracy
    }

    public static Single Clamp01(Single fval) {
        int s;
        _ifVal.f = fval;
        s = _ifVal.i >> 31; // all 1's if sign bit 1
        _ifVal.i &= ~s; // 0 if was negative

        // clamp to 1
        _ifVal.f = 1.0f - _ifVal.f;
        s = _ifVal.i >> 31; // all 1's if sign bit 1
        _ifVal.i &= ~s; // 0 if was negative
        _ifVal.f = 1.0f - _ifVal.f;

        //System.Diagnostics.Debug.Assert( ( 0 <= _ifVal.f ) && ( _ifVal.f <= 1.0f ) );
        return _ifVal.f;
    }
    public static Single ClampAB(Single fval, Single A, Single B) {
        System.Diagnostics.Debug.Assert(A < B); // todo - make asserts have message also
        fval -= A;
        fval /= (B - A);
        fval = Clamp01(fval);
        fval *= (B - A);
        fval += A;
        System.Diagnostics.Debug.Assert((A <= fval) && (fval <= B));
        return fval;
    }

    public static Single ClampNonnegative(Single fval) {
        _ifVal.f = fval;
        int s = _ifVal.i >> 31; // all 1's if sign bit 1
        _ifVal.i &= ~s; // 0 if was negative
        System.Diagnostics.Debug.Assert(0 <= _ifVal.f);
        return _ifVal.f;
    } // Clamp01

    public static Int32 FloatToInt(Single fval) {
        System.Diagnostics.Debug.Assert(-4194303 <= fval && fval <= 4914303);
        // float version goes as follows - the double upcast gives better range - time for faster?
        // convert
        fval += (1 << 23) + (1 << 22);
        _ifVal.f = fval;
        _ifVal.i &= (1 << 23) - 1; // mask out
        _ifVal.i -= (1 << 22);
        return _ifVal.i;
    }
    
    public static float IntToFloat(int ival) {
        System.Diagnostics.Debug.Assert(-4194303 <= ival && ival <= 4914303);
        IntOrFloat val = new IntOrFloat(0);
        IntOrFloat bias = new IntOrFloat(0);
        val.i = ival;
        bias.i = ((23 + 127) << 23) + (1 << 22);
        val.i += bias.i;
        val.f -= bias.f;
        return val.f;
    }

    public static int IntLog2(float fval) {
        System.Diagnostics.Debug.Assert(fval > 0);
        _ifVal.f = fval;
        return (_ifVal.i >> 23) - 127; // strip out exponent, remove sign, unbias
    }
    
    public static float CalculatePitch(Vector3 dir) {
        Vector3 level = dir;
        level.y = 0.0f;

        level.Normalize();
        dir.Normalize();
        float pitch = Vector3.Dot(level, dir);
        if (pitch > 1.0f) {
            pitch = 1.0f;
        }
        if (pitch < -1.0f) {
            pitch = -1.0f;
        }
        pitch = Mathf.Acos(pitch);

        if (dir.y > 0.0f) {
            pitch = Mathf.PI * 2 - pitch;
        }
        return pitch;
    }


    public static float CalculateYaw(Vector3 dir) {
        dir.y = 0.0f;
        dir = Vector3.Normalize(dir);
        Vector3 z = new Vector3(0.0f, 0.0f, 1.0f);
        float yaw = Vector3.Dot(z, dir);
        if (yaw > 1.0f) {
            yaw = 1.0f;
        }
        if (yaw < -1.0f) {
            yaw = -1.0f;
        }
        yaw = Mathf.Acos(yaw);
        if (dir.x < 0.0f) {
            yaw = Mathf.PI * 2.0f - yaw;
        }
        return yaw;
    }
    
    public static float NormalizeAngle(float degree) {
        degree %= 360.0f;
        if (degree < 0) {
            degree += 360.0f;
        }
        return degree;
    }

    public static float NormalizeRadian(float radian) {
        radian %= PI2;
        if (radian < 0) {
            radian += PI2;
        }
        return radian;
    }

    public static Vector3 ToVector3_XOZ(ref Vector2 point) {
        return new Vector3(point.x, 0, point.y);
    }

    public static Vector2 ToVector2_XOZ(Vector3 point) {
        return new Vector2(point.x, point.z);
    }

    public static Vector2 ToVector2_XOZ(ref Vector3 point) {
        return new Vector2(point.x, point.z);
    }
    
    public static Vector3 GetDirection_XOZ_Fast(ref Vector3 start, ref Vector3 end) {
        float a = (end.x - start.x);
        float b = (end.z - start.z);
        float lenSq = a * a + b * b;
        if (lenSq <= float.Epsilon) {
            return Vector3.left;
        }
        float rlen = InvSqrt(lenSq);
        return new Vector3(a * rlen, 0, b * rlen);
    }

    public static Vector3 GetDirection_XOZ(ref Vector3 start, ref Vector3 end) {
        float a = (end.x - start.x);
        float b = (end.z - start.z);
        float lenSq = a * a + b * b;
        if (lenSq <= float.Epsilon) {
            return Vector3.left;
        }
        float rlen = 1.0f / Mathf.Sqrt(lenSq);
        return new Vector3(a * rlen, 0, b * rlen);
    }

    public static Vector3 GetDirection_XOZ(Vector3 start, Vector3 end) {
        return GetDirection_XOZ(ref start, ref end);
    }
    
    public static Vector2 GetDirectionFast(ref Vector2 start, ref Vector2 end) {
        float a = (end.x - start.x);
        float b = (end.y - start.y);
        float lenSq = a * a + b * b;
        if (lenSq <= float.Epsilon) {
            return Vector2.one;
        }
        float rlen = InvSqrt(lenSq);
        return new Vector2(a * rlen, b * rlen);
    }

    public static Vector2 GetDirection(ref Vector2 start, ref Vector2 end) {
        float a = (end.x - start.x);
        float b = (end.y - start.y);
        float lenSq = a * a + b * b;
        if (lenSq <= float.Epsilon) {
            return Vector2.one;
        }
        float rlen = 1.0f / Mathf.Sqrt(lenSq);
        return new Vector2(a * rlen, b * rlen);
    }

    public static Vector2 GetDirection(Vector2 start, Vector2 end) {
        return GetDirection(ref start, ref end);
    }

    public static float GetDistance_XOZ(Vector3 start, Vector3 end) {
        float a = (end.x - start.x);
        float b = (end.z - start.z);
        return Mathf.Sqrt(a * a + b * b);
    }
    
    public static float GetDistance_XOZ(ref Vector3 start, ref Vector3 end) {
        float a = (end.x - start.x);
        float b = (end.z - start.z);
        return Mathf.Sqrt(a * a + b * b);
    }

    public static float GetDistance_XOZ_Fast(Vector3 start, Vector3 end) {
        float a = (end.x - start.x);
        float b = (end.z - start.z);
        return FastSqrt1(a * a + b * b);
    }

    public static float GetDistance_XOZ_Fast(ref Vector3 start, ref Vector3 end) {
        float a = (end.x - start.x);
        float b = (end.z - start.z);
        return FastSqrt1(a * a + b * b);
    }

    public static float GetDistanceSq_XOZ(Vector3 start, Vector3 end) {
        float a = (end.x - start.x);
        float b = (end.z - start.z);
        return a * a + b * b;
    }

    public static float GetDistanceSq_XOZ(ref Vector3 start, ref Vector3 end) {
        float a = (end.x - start.x);
        float b = (end.z - start.z);
        return a * a + b * b;
    }

    public static float GetDistance(Vector2 start, Vector2 end) {
        float a = (end.x - start.x);
        float b = (end.y - start.y);
        return Mathf.Sqrt(a * a + b * b);
    }

    public static float GetDistance(ref Vector2 start, ref Vector2 end) {
        float a = (end.x - start.x);
        float b = (end.y - start.y);
        return Mathf.Sqrt(a * a + b * b);
    }

    public static float GetDistanceFast(Vector2 start, Vector2 end) {
        float a = (end.x - start.x);
        float b = (end.y - start.y);
        return FastSqrt1(a * a + b * b);
    }

    public static float GetDistanceSq(ref Vector2 start, ref Vector2 end) {
        float a = (end.x - start.x);
        float b = (end.y - start.y);
        return a * a + b * b;
    }
    
    public static float GetDistanceSq(Vector2 start, Vector2 end) {
        float a = (end.x - start.x);
        float b = (end.y - start.y);
        return a * a + b * b;
    }

    public static UInt32 Right_PowerOf2(UInt32 v) {
        UInt32 t = 1;
        while (t < v) {
            t <<= 1;
        }
        return t;
    }

    public static float VectorDot(ref Vector2 a, ref Vector2 b) {
        return a.x * b.x + a.y * b.y;
    }

    public static float VectorDot(ref Vector3 a, ref Vector3 b) {
        return a.x * b.x + a.y * b.y + a.z * b.z;
    }

    public static float VectorDot(ref Vector4 a, ref Vector4 b) {
        return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
    }

    public static Vector3 RandomVector3() {
        var v = new Vector3(UnityEngine.Random.Range(-1.0f, 1.0f), UnityEngine.Random.Range(-1.0f, 1.0f), UnityEngine.Random.Range(-1.0f, 1.0f));
        v.Normalize();
        return v;
    }

    public static Vector2 RandomVector2() {
        var v = new Vector2(UnityEngine.Random.Range(-1.0f, 1.0f), UnityEngine.Random.Range(-1.0f, 1.0f));
        v.Normalize();
        return v;
    }
    
    public static float CalculateRatio(int currentNum, int fullNum) {
        if (fullNum == 0) {
            return 0;
        }

        float temp = (float)currentNum / fullNum;
        temp = Mathf.Clamp(temp, 0, 1);
        return temp;
    }

    public static UInt64 MakeUInt32ToUInt64High(UInt64 value) {
        return value << 32;
    }

    public struct Matrix3x3
    {
        public float m00;
        public float m01;
        public float m02;
        public float m10;
        public float m11;
        public float m12;
        public float m20;
        public float m21;
        public float m22;
    }
    
    public static void ToMatrix3(ref Matrix3x3 o, ref Quaternion q) {
        float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
        x2 = q.x + q.x;
        y2 = q.y + q.y;
        z2 = q.z + q.z;
        xx = q.x * x2;
        xy = q.x * y2;
        xz = q.x * z2;
        yy = q.y * y2;
        yz = q.y * z2;
        zz = q.z * z2;
        wx = q.w * x2;
        wy = q.w * y2;
        wz = q.w * z2;

        o.m00 = 1 - (yy + zz);
        o.m10 = xy + wz;
        o.m20 = xz - wy;
        o.m01 = xy - wz;
        o.m11 = 1 - (xx + zz);
        o.m21 = yz + wx;
        o.m02 = xz + wy;
        o.m12 = yz - wx;
        o.m22 = 1 - (xx + yy);
    }

    public static void ToMatrix4(ref Matrix4x4 o, ref Quaternion q) {
        float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
        x2 = q.x + q.x;
        y2 = q.y + q.y;
        z2 = q.z + q.z;
        xx = q.x * x2;
        xy = q.x * y2;
        xz = q.x * z2;
        yy = q.y * y2;
        yz = q.y * z2;
        zz = q.z * z2;
        wx = q.w * x2;
        wy = q.w * y2;
        wz = q.w * z2;

        o.m00 = 1 - (yy + zz);
        o.m10 = xy + wz;
        o.m20 = xz - wy;
        o.m30 = 0;
        o.m01 = xy - wz;
        o.m11 = 1 - (xx + zz);
        o.m21 = yz + wx;
        o.m31 = 0;
        o.m02 = xz + wy;
        o.m12 = yz - wx;
        o.m22 = 1 - (xx + yy);
        o.m32 = 0;
        o.m03 = 0;
        o.m13 = 0;
        o.m23 = 0;
        o.m33 = 1;
    }
    
    public static void Mul(ref Matrix3x3 o, ref Matrix3x3 m1, ref Matrix3x3 m2) {
        o.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20;
        o.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21;
        o.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22;
        o.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20;
        o.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21;
        o.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22;
        o.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20;
        o.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21;
        o.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22;
    }

    public static void Mul(ref Matrix4x4 o, ref Matrix4x4 m1, ref Matrix4x4 m2) {
        o.m00 = m1.m00 * m2.m00 + m1.m01 * m2.m10 + m1.m02 * m2.m20 + m1.m03 * m2.m30;
        o.m01 = m1.m00 * m2.m01 + m1.m01 * m2.m11 + m1.m02 * m2.m21 + m1.m03 * m2.m31;
        o.m02 = m1.m00 * m2.m02 + m1.m01 * m2.m12 + m1.m02 * m2.m22 + m1.m03 * m2.m32;
        o.m03 = m1.m00 * m2.m03 + m1.m01 * m2.m13 + m1.m02 * m2.m23 + m1.m03 * m2.m33;
        o.m10 = m1.m10 * m2.m00 + m1.m11 * m2.m10 + m1.m12 * m2.m20 + m1.m13 * m2.m30;
        o.m11 = m1.m10 * m2.m01 + m1.m11 * m2.m11 + m1.m12 * m2.m21 + m1.m13 * m2.m31;
        o.m12 = m1.m10 * m2.m02 + m1.m11 * m2.m12 + m1.m12 * m2.m22 + m1.m13 * m2.m32;
        o.m13 = m1.m10 * m2.m03 + m1.m11 * m2.m13 + m1.m12 * m2.m23 + m1.m13 * m2.m33;
        o.m20 = m1.m20 * m2.m00 + m1.m21 * m2.m10 + m1.m22 * m2.m20 + m1.m23 * m2.m30;
        o.m21 = m1.m20 * m2.m01 + m1.m21 * m2.m11 + m1.m22 * m2.m21 + m1.m23 * m2.m31;
        o.m22 = m1.m20 * m2.m02 + m1.m21 * m2.m12 + m1.m22 * m2.m22 + m1.m23 * m2.m32;
        o.m23 = m1.m20 * m2.m03 + m1.m21 * m2.m13 + m1.m22 * m2.m23 + m1.m23 * m2.m33;
        o.m30 = m1.m30 * m2.m00 + m1.m31 * m2.m10 + m1.m32 * m2.m20 + m1.m33 * m2.m30;
        o.m31 = m1.m30 * m2.m01 + m1.m31 * m2.m11 + m1.m32 * m2.m21 + m1.m33 * m2.m31;
        o.m32 = m1.m30 * m2.m02 + m1.m31 * m2.m12 + m1.m32 * m2.m22 + m1.m33 * m2.m32;
        o.m33 = m1.m30 * m2.m03 + m1.m31 * m2.m13 + m1.m32 * m2.m23 + m1.m33 * m2.m33;
    }

    public static void CreateFromSRT(ref Matrix4x4 rotM, ref Vector3 scale, ref Quaternion rot, ref Vector3 trans) {
        ToMatrix4(ref rotM, ref rot);
        rotM.m00 *= scale.x;
        rotM.m10 *= scale.x;
        rotM.m20 *= scale.x;
        rotM.m01 *= scale.y;
        rotM.m11 *= scale.y;
        rotM.m21 *= scale.y;
        rotM.m02 *= scale.z;
        rotM.m12 *= scale.z;
        rotM.m22 *= scale.z;
        rotM.m03 += trans.x;
        rotM.m13 += trans.y;
        rotM.m23 += trans.z;
    }
    
    public static void CreateFromSRT(ref Matrix4x4 rotM, ref Quaternion rot, ref Vector3 trans) {
        ToMatrix4(ref rotM, ref rot);
        rotM.m03 += trans.x;
        rotM.m13 += trans.y;
        rotM.m23 += trans.z;
    }

    public static void CreateFromSRT(ref Matrix4x4 rotM, ref Vector3 scale, ref Quaternion rot) {
        ToMatrix4(ref rotM, ref rot);
        rotM.m00 *= scale.x;
        rotM.m10 *= scale.x;
        rotM.m20 *= scale.x;
        rotM.m01 *= scale.y;
        rotM.m11 *= scale.y;
        rotM.m21 *= scale.y;
        rotM.m02 *= scale.z;
        rotM.m12 *= scale.z;
        rotM.m22 *= scale.z;
    }

    public static void TransformCoord(ref Vector3 o, ref Matrix4x4 mat, ref Vector3 p) {
        o.x = p.x * mat.m00 + p.y * mat.m01 + p.z * mat.m02 + mat.m03;
        o.y = p.x * mat.m10 + p.y * mat.m11 + p.z * mat.m12 + mat.m13;
        o.z = p.x * mat.m20 + p.y * mat.m21 + p.z * mat.m22 + mat.m23;
    }

    public static Vector3 TransformCoord(ref Matrix4x4 mat, ref Vector3 p) {
        return new Vector3(p.x * mat.m00 + p.y * mat.m01 + p.z * mat.m02 + mat.m03,
            p.x * mat.m10 + p.y * mat.m11 + p.z * mat.m12 + mat.m13,
            p.x * mat.m20 + p.y * mat.m21 + p.z * mat.m22 + mat.m23);
    }

    public static Vector3 TransformCoordXY(ref Matrix4x4 mat, ref Vector3 p) {
        return new Vector3(p.x * mat.m00 + p.y * mat.m01 + mat.m03,
            p.x * mat.m10 + p.y * mat.m11 + mat.m13,
            p.x * mat.m20 + p.y * mat.m21 + mat.m23);
    }
}

public class LinearInterpolator
{
    float dest = 0;
    float cur = 0;
    float speed = 0;

    public float Dest {
        get {return dest;}
        set {dest = value;}
    }
    public float Cur {
        get {return cur;}
        set {cur = value;}
    }
    public float Speed {
        get {return speed;}
        set {speed = Math.Abs(value); }
    }

    public LinearInterpolator(float speed) { this.Speed = speed; }
    public bool IsRunning() { return Cur != Dest; }
    public void Update(float dt) {
        if (Dest == Cur) {
            return;
        }
        var sign = Math.Sign(Dest - Cur);
        float newValue = Cur + Speed * sign * dt;
        if ((newValue - Dest) * (Cur - Dest) > 0) {
            Cur = newValue;
        } else {
            Cur = Dest;
        }
    }
}

